package sep.test;

import static com.googlecode.javacv.cpp.opencv_core.CV_32FC1;
import static com.googlecode.javacv.cpp.opencv_core.cvLine;
import static com.googlecode.javacv.cpp.opencv_core.cvSetIdentity;

import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_core.CvPoint;
import com.googlecode.javacv.cpp.opencv_core.CvPoint2D32f;
import com.googlecode.javacv.cpp.opencv_core.IplImage;

public class IdaPolynom3Street extends IdaStreetModel {

	private static final long serialVersionUID = -2865566076311089052L;
	private final static double DRAW_STEP_WIDTH = MAX_STREET_LENGTH / 10.0;

	public IdaPolynom3Street() {
		create();
	}

	@Override
	public void drawLane(final IplImage image) {
		if (image == null) {
			throw new IllegalArgumentException("Image must not be null.");
		}

		final double halfWidth = matState.get(0, 0) / 2.0;
		final double offset = matState.get(1, 0);
		final double k1 = matState.get(2, 0);
		final double k2 = matState.get(3, 0);
		final double k3 = matState.get(4, 0);
		final CvPoint[] left = new CvPoint[(int) (MAX_STREET_LENGTH / DRAW_STEP_WIDTH)];
		final CvPoint[] right = new CvPoint[left.length];

		double l = 0.0;
		double y;
		CvPoint2D32f p;

		for (int idx = 0; idx < left.length; idx++) {
			y = -offset + k1 * l + k2 * l * l + k3 * l * l * l;
			p = cameraModel.toCameraCoordinates(l, -halfWidth + y, 0.0);
			left[idx] = new CvPoint((int) p.x(), (int) p.y());
			p = cameraModel.toCameraCoordinates(l, halfWidth + y, 0.0);
			right[idx] = new CvPoint((int) p.x(), (int) p.y());

			l += DRAW_STEP_WIDTH;
		}

		for (int i = 0; i < left.length - 1; i++) {
			cvLine(image, left[i], left[i + 1], LANE_COLOR, 2, 8, 0);
			cvLine(image, right[i], right[i + 1], LANE_COLOR, 2, 8, 0);
		}
	}

	@Override
	public double getIntersectionPoint(final int scanlineIdx,
			final boolean leftPoint) {
		if (!initialized) {
			throw new IllegalStateException(
					"Street model must be initialized first.");
		}
		if (scanlineIdx < 0 || scanlineIdx >= scanlines.length) {
			throw new IllegalArgumentException("Scanline index does not exist.");
		}

		// Strassenbegrenzung mit Scanline schneiden
		// x-Wert ist scanlines[scanlineIdx]
		final double halfWidth = matState.get(0, 0) / 2.0;
		final double offset = matState.get(1, 0);
		final double k1 = matState.get(2, 0);
		final double k2 = matState.get(3, 0);
		final double k3 = matState.get(4, 0);
		final double l = scanlines[scanlineIdx];
		double yVal;

		if (leftPoint) {
			yVal = -halfWidth - offset + k1 * l + k2 * l * l + k3 * l * l * l;
		} else {
			yVal = halfWidth - offset + k1 * l + k2 * l * l + k3 * l * l * l;
		}

		// Es wird nur der y-Wert gebraucht
		return yVal;
	}

	@Override
	public StreetModelType getStreetType() {
		return StreetModelType.Polynom3;
	}

	@Override
	public CvMat getObservationMatrix(final boolean leftPoint,
			final int scanlineIdx) {
		// Beobachtungsmatrix ist:
		// +-0.5, -1.0, l, l�, l�

		if (leftPoint) {
			matObservation.put(0, 0, -0.5);
		} else {
			matObservation.put(0, 0, 0.5);
		}
		matObservation.put(0, 1, -1.0);

		final double l = scanlines[scanlineIdx];

		matObservation.put(0, 2, l);
		matObservation.put(0, 3, l * l);
		matObservation.put(0, 4, l * l * l);

		return matObservation;
	}

	@Override
	public void reset() {
		matState.put(0, 0, INITIAL_WIDTH);
		matState.put(1, 0, 0.0);
		matState.put(2, 0, 0.0);
		matState.put(3, 0, 0.0);
		matState.put(4, 0, 0.0);
	}

	@Override
	public void create() {
		// Status: Polynom (k1x + k2x� + k3x�)
		// (Strassenbreite)
		// (Offset zur Fahrzeugmitte)
		// (k1)
		// (k2)
		// (k3)
		// x Achse: in Fahrtrichtung
		// y Achse: Strassenbreite
		matState = CvMat.create(5, 1, CV_32FC1);

		// Beobachtungsmatrix
		matObservation = CvMat.create(1, 5, CV_32FC1);

		// �bergangsmatrix
		matTransition = CvMat.create(5, 5, CV_32FC1);
		cvSetIdentity(matTransition);
	}

}
